#include "RosInterface.h"

/*! \fn void *AuxControlThread(void *threadParam)
 * \param threadParam as void *, parameters of thread
 * Function executing in the thread
*/
void *AuxControlThread(void *threadParam){
	RosInterface *ComponentThread = (RosInterface *)threadParam;
	ComponentThread->ControlThread();
    pthread_detach(pthread_self());
	return NULL;
}

RosInterface::RosInterface()
{
    turtleVelocity.linear = turtleVelocity.angular = 0.0;

    sub = n.subscribe("/turtle1/pose", 1000, &RosInterface::turttleposeCallback, this);
    pub = n.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1000);
    client_clear = n.serviceClient<std_srvs::Empty>("clear");
}

RosInterface::~RosInterface()
{
    //dtor
}

int RosInterface::Start()
{
    pthread_attr_t attr;	// Thread attributed for the component threads spawned in this function
    //sprintf(cAux,"%s::Start: launching the thread", sComponentName.c_str());
    //rlcLog->AddEvent(cAux);

    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
    // Creation of the main thread
    if(pthread_create(&threadData.pthreadId, &attr, AuxControlThread, this) != 0)	{
        printf("Start: Could not create ControlThread");

        pthread_attr_destroy(&attr);
        return -1;
    }

    pthread_attr_destroy(&attr);
    return 0;
}

void RosInterface::turttleposeCallback(const turtlesim::Pose::ConstPtr& msg)
{
    turtlePose.x = msg->x;
    turtlePose.y = msg->y;
    turtlePose.theta = msg->theta;
    turtlePose.linear_velocity = msg->linear_velocity;
    turtlePose.angular_velocity = msg->angular_velocity;

    ROS_INFO("I heard: [x, y, theta] = [%f, %f, %f] [v = %f, w = %f]\n", msg->x, msg->y, msg->theta, msg->linear_velocity, msg->angular_velocity);
}

void RosInterface::ControlThread(){
    ros::spin();
}

turtlesim::Pose RosInterface::GetTurtlePose(){
    return turtlePose;
}
